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A Small AUV Navigation System (SANS) is being developed at the Naval 
Postgraduate School. The SANS is an integrated INS/GPS navigation system composed 
of low-cost, small-size components. It is designed to demonstrate the feasibility of using 
a low-cost Inertial Measurement Unit (IMU) to navigate between intermittent GPS fixes. 

This thesis presents recent improvements to the SANS hardware and software. 
The 486-based ESP computer used in the previous version of SANS is now replaced by 
an AMD 586DX133 based PC/104 computer to provide more computing power, 
reliability and compatibility with PC/104 industrial standards. The previous SANS 
navigation filter consisting of a complementary constant gain filter is now aided by an 
asynchronous Kalman filter. This navigation filter has six states for orientation 
estimation (constant gain) and eight states for position estimation (Kalman filtered). 
Low-frequency DGPS noise is explicitly modeled based on an experimentally obtained 
autocorrelation function. Ocean currents are also modeled as a low-frequency random 
process. The asynchronous nature of DGPS measurements resulting from AUV 
submergence or wave splash on the DGPS antennas is also taken into account by 
adopting an asynchronous Kalman filter as the basis for the SANS software. Matlab 
simulation studies of the asynchronous filter have been conducted and results 


documented in this thesis. 


I]. 


( 


TABLE OF CONTENTS 


JOSPID LGU DAOC TION oc doce ke ee ease l 
Jeet EIN SI RUE IL, ahh o00.50 <0 RNa ena See so ] 
Jake JEVAN CIS GIRS OOS] BT. oe oi tae On eee) Oe | 
Cram era CIN TA Soa ic cncnceeis dee vendew ee neteuieneveconsadsdacneeess y 
DeSeOre EUill ATIONS, AND ASSUMPTIONS. ........cccccccieerceecss cscs secon ees 3 
IB, CO GUINEA ye ILC ING) ole Bl gf Sins) IS geese ak ee seo Penne S52 coo dene 3 
SAIN S Pee ORNIORIOMECA TION 22 ......c:c0.:cccc00-0cbehaasusecsisessemansecahesvoepsaeiiveresora: 5 
Fe eg UNE RCO LILA IO. lg ARES IR rac re ea 2) 
ENE le MINE CV TZ NOEOUIN: DELEON Crrcctmir sts sncrccrrresccrccrettearterey ete necrereecsetres 5 
C. REAL WORLD PROBLEMS WITH INERTIAL NAVIGATION ................... 5 
BIS ANS NAVIGATION | eiec ooo oi ccessvecacntecdeciscalu tind sc. Mos Oe es 5 7 
SPINS) SN Ge c Heh LCL Geer Gi Oy 27259 UG Is Rae ste 9 
J INTURCOI DGS LG ances apne ene se eee 9 
8), TERING EISy Sy UN fl E881) Ke) hana oe 9 
Gy WISI SINE SIAN IS I g 02800 B92 0.0 Seen 10 
BE ree LULU ES IN Sis eID VV RRS oy ee cccc cic eeensanedsadu-0<esssencanasdeseasadedsaaecseieagve: 14 


Vil 


IV. SANS ASYNCHRONOUS KARMA RID RD Roe ee eee ie 


A. INTRODUCTION oo. re essere 17 
B.. KALMAN FILTER BASI@S er eee sees) -caesdis 17 
C. DERIVATION OF SANS KALMAN FILTER EQUATIONS....................... Al 
D. SANS KALMAN FILTER SIMULATION ........ cee Reins: ee 24 
V.. CONCLUSIONS ....).2.. =a ee 29 
Poo SUMMARY 1 ic.3. cans ecee eres erage tetas d ind see nae mec er ice US) 
B. FUTURE RESEAR C ern saccsceetes ess ea anette nee eer 29 


APPENDIX A. SANS ASYNCHRONOUS KALMAN FILTER MATLAB CODE... 31 


APPENDIX B. ERROR COVARIANCE MATRIX FOR SANS ASYNCHRONOUS 


KALMAN FIL TERS ye ON ieee ate, og 
USOC RE EERE NCES cig eee 0s psiloc caste em ae «Serene So 
NOE SRDS TRIB UTI CIN ACIS Air sce 25, cvsmaerner ce eee eee nari e eh cc ear eae 4] 


Vill 


Zr) 


Dee 


| 


See 


4.1 


4.2 


4.3 


4.4 


45 


LIST OF FIGURES 


Previous SANS Filter 


Current SANS Filter 


*SSSSSSSHSSSSSSSESSSESSSESSSSESSSESSSHSSEHSEEFSSEHSFSESEESSH 


Plot of Estimated North Position vs Estimated East Position 


SSSCSSSSSESSESEGeetSEEEGGesseeeeeeusegega 


PT eRe REOCEPOreree errr e err rerrerererrerrer reer eee eee rere eee Eee erererrerrrrrere re yy ey re 


SSSSSSSSSSSSSHSSHSSHSSSHSHSSHSSSSSSESSSSSFHSSSSFFeeeFFSFSSFESSFeeFFFFeeeFFSSSSFFesseessSSSSsFSSSGseeseseeetennssangaa 


20 


20 


5) 


26 


Pad 





ACKNOWLEDGEMENTS 


First and foremost, | would like to thank Dr. Xiaoping Yun for guiding me 
through this enlightening experience. His patience and understanding gave me the 
motivation to keep moving forward and use common sense. I am much appreciative of 
all the support he has given to my efforts on the SANS project. Secondly, many thanks 
go to Dr. Robert McGhee and Eric Bachmann for their many insights during my research. 
Their breaths of knowledge never cease to amaze me. Finally, I would like to express my 
gratitude to Russ Whalen for trying to show me six years worth of knowledge and work 
on SANS hardware within a span of two weeks. His presence was sorely missed, but I 
appreciate the bay view office you left me, Thanks Russ. 

This research was supported in part by Grant CDA-9729814 from the National 


Science Foundation to the Naval Postgraduate School. 


XI 





DEDICATION 


For Mom and Dad 


Xiii 





I. INTRODUCTION 


A. GENERAL 

In vehicle navigation systems, integrated inertial measurement devices are 
commonly used to compensate for the loss of Global Positioning Systems (GPS) signals 
due to environmental blockages. These devices have been in use for some time in 
aircraft, ships, and large land vehicles. In the past, inertial information has been derived 
from expensive gyros that required large spaces relative to the size of the vehicle. As the 
size of the vehicles gets smaller, implementing these kinds of integrated devices can be 
quite expensive. Thus, research is being conducted on ways to compensate for this GPS 
signal loss with low cost off-the-shelf components. Current chip technology involving 
micromachined silicon has allowed progress in this particular area of navigation systems. 
The goal of this thesis is to show that a low cost integrated (Inertial Navigiation System) 
INS/GPS navigation system can be developed using commercially available components. 
B. BACKGROUND 

At the Naval Postgraduate School (NPS), a small autonomous underwater vehicle 
(AUV) known as Phoenix requires accurate navigation to report the position of shallow 
water mines or monitor the coastal environment [Ref. 3]. In the course of its missions, 
the vehicle periodically surfaces to obtain a Differential Global Positioning System 
(DGPS) fix and then submerges. While underwater, the Phoenix uses gyros, a depth 
sensor, a speed sensor, and compass heading to estimate its position. Once a shallow 
water mine has been detected with sonars, the location of the mine can be estimated 


based on the position of the AUV. Previous hardware and software were tested with 
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promising results. The current Phoenix navigation system has two faults. One, it does 
not account for water current, and two, its gyros are costly, bulky, power hungry, and 
prone to failure. 

Over the last seven years, research has gone into solving these problems and the 
Small AUV Navigation System (SANS) is the outcome of this work. SANS, now in its 
third generation of development, has undergone a number changes [Ref. 3]. 

SANS technology is also becoming a more attractive solution to human motion 
tracking. Since the size of its components is getting smaller with each generation, 
improved portability is making it possible to track a human carrying a SANS device. For 
example, a small law enforcement team carrying SANS might enter a building with their 
location being sent back to a remote location outside the building so tacticians could 
better evaluate situations. 

As suggested in the previous examples, the application of a device such as SANS 
is limitless. In this thesis, work has been narrowed to AUV navigation, at the same time 
knowing that applications beyond Phoenix are possible. 

@ DESIGN TASKS 
e Replace SANS computer; 
e Replace Inertial Measurement Unit; 
e Replace DGPS unit; 
e Replace magnetometer unit; 
e Replace speed sensor; 
e Develop an asynchronous Kalman filter for SANS; 


e Conduct simulation of the asynchronous Kalman filter for SANS; 
2 


e Test and evaluate any new components. 
D. SCOPE, LIMITATIONS, AND ASSUMPTIONS 

This thesis reports part of the findings of the seventh year of research in an 
ongoing research project. A new hardware configuration for SANS and a simulation of 
the SANS asynchronous Kalman filter are the main outcomes of this work. The scope of 
this thesis is to develop a new navigation system (SANS) for eventual installation as a 
replacement for the expensive and older technology gyros now used onboard the NPS 
Been AUV. 
| By ORGANIZATION OF THESIS 

The purpose of this thesis is to present a prototype hardware platform and the 
filter theory required to meet the mission requirements of SANS. In this thesis, the term 
AUV is understood to include any small underwater vehicle (including humans) which 
can easily carry such a compact device (SANS). The major thrust of this thesis 1s to 
describe the latest developments of the SANS hardware and offer an optimal filter as a 
replacement to the original complementary filter. 

Chapter II presents an overview of the SANS theory of operation and an 
explanation of the navigation filter. Chapter III presents the previous SANS hardware 
configuration and its current state. Chapter IV reviews Kalman filter basics, describes 
the SANS asynchronous Kalman filter equations and presents simulation results. Chapter 


V presents the thesis conclusions and provides recommendations for future research. 





II. SANS THEORY OF OPERATION 


ne INTRODUCTION 

The objective of the SANS 1s to produce real time navigation information by 
integrating an Inertial Navigational System with Differential Global Positioning System 
(DGPS). Previous work on the SANS filter discussed in [Ref. 3,5 and 6] consists of a 14- 
state complementary filter designed to estimate north-east position. This chapter Pe 
the theory of inertial navigation, real world problems involved with this type of 
navigation, and how SANS uses this theory to deliver navigation information. 
B. INERTIAL NAVIGATION THEORY 

To navigate between GPS fixes, SANS uses inertial sensors that measure linear 
accelerations in three orthogonal axes and angular rates about three orthogonal axes. Ina 
perfect world where errors do not occur in sensors, angular rate could be integrated to 
give angular position or orientation. Integrating the acceleration twice would give 
position. Thus, position could be obtained in three dimensions given acceleration in three 
orthogonal axes. Theoretically, with no noise in sensor measurement, orientation and 
position could be found using only an Inertial Measurement Unit (IMU) given an initial 
attitude and position. 
C. REAL WORLD PROBLEMS WITH INERTIAL NAVIGIA TION 

In practice, low cost angular rate sensors and linear accelerometers are prone to 
drift. Estimating position and orientation with an IMU requires knowledge of the drift 
characteristics of the sensors and how to correct them. Using sensors that are more 


accurate drives costs into the tens of thousands of dollars. Thus, a method is needed to 
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account for sensor drift. Angular rate sensor data are used as high frequency data and 
integrated once to obtain orientation information in a short period of time. Accelerometer 
data are used as low frequency data to correct drift in orientaion estimation. [Ref. 2] 

Integrating linear acceleration twice gives linear position. Integration causes 
errors 1n position estimation to grow quadratically. DGPS and waterspeed sensors are 
used as low frequency devices to correct drift in position estimation. Once acceleration is 
integrated to give velocity, errors within this integration is corrected with waterspeed. 
When velocity is integrated to give position, DGPS ts used correct position errors. 


Differences between DGPS position and IMU position are attributed to ocean current. 
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Figure 2.1: Previous SANS Filter 


D. SANS NAVIGATION 

SANS uses the principles of inertial navigation to estimate attitude. GPS and 
waterspeed data aid in position estimation. The previous SANS filter (Figure 2.1) is a 
14-state complementary filter. Attitude estimation transforms all the body rates and 
angles of the AUV to earth-based coordinates, namely, north-east-down. The T 
transformation matrix in Figure 2.1 converts body angular rates to Euler rates and the R 
tranformation matrix transforms body-based linear accelerations and velocities to earth- 


based angles. Six states account for attitude, three for angular rate and three for 
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acceleration. The other eight states are north-east position, velocity, ocean current, and 
GPS bias. Position in the down axis is obtained using a pressure sensor. This thesis only 
focuses on the north-east coordinate estimation. The eight states were determined using 
constant gains for error correction. 

SANS obtains accurate navigation information by integrating IMU data and 
DGPS data. While IMU data is sampled periodically, DGPS is available aperiodically 
due to asynchronous reacquisition time of satellite signals and asynchronous 
submergence-surfacing duration of the AUV. To optimally integrate IMU and DGPS 


data, an asynchronous Kalman filter is an ideal method. 
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Figure 2.2: Current SANS Filter 





Figure 2.2 shows the how eight states would be used in an asynchronous Kalman filter. 
The IMU and compass give the necessary Euler angles to break waterspeed into north- 
east vector using the AUV body to earth based coordinate R rotation matrix. Chapter IV 


goes into more detail as to how the filter was developed. 


Ill. SANS SYSTEM CONFIGURATION 


A. INTRODUCTION 

The objective of SANS is to develop a low cost way to navigate between DGPS 
fixes using an IMU. The system should be small enough to fit into the Phoenix AUV. 
References [2,3,5,6, and 7] described the previous SANS. This thesis presents the current 
SANS. Most parts of the previous unit have been replaced with cheaper, smaller, and 
more powerful components. This chapter briefly describes the previous SANS and then 
details the current SANS configuration and the operation. 
B. PREVIOUS SANS HARDWARE 

The previous SANS (Figure 3.1) consisted of a 486SLC DX2 CPU module, an 
A/D converter, Systron Donner Inertial Measurement Unit (IMU), a waterspeed sensor , a 
Motorola ONCORE 8-channel receiver with an imbedded DGPS capability, and a 
Precision Navigation TCM2 Electronic Compass. These components were installed in a 
box measuring 6 x 17 x 3 inches (306 cubic inches). The previous version of SANS 
sampled at 40 Hz and required 12 VDC power. The electronic compass and power 


supply were located external to the box to reduce magnetic interference. 
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G: CURRENT SANS HARDWARE 

The current SANS configuration (Figure 3.2) is smaller, faster, and more flexible. 
The DGPS and electronic compass were kept from the old system. All other components 
were upgraded. This version of SANS is capable of sampling at an estimated rate of 100 
Hz and requires only 5 VDC to operate. The current SANS configuration is 
approximately 150 cubic inches excluding the electronic compass and the power supply. 
The new components are a Real Time Devices 133 MHz AMD 586 PC/104, a Sealevel 
four port RS-232 module, a Crossbow six axis inertial measurement unit, and a SonTek 


Hydra water speed sensor. 
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Figure 3.2: Current SANS Hardware Configuration 


1. Real Time Devices DSVS586DX133 PC/104 

The Real Time Devices PC/104 module will process all data received from the 
sensors. Utilizing to the PC/104 standard adds flexibility to the system. The PC/104 
CPU is small, relatively cheap at $600 per CPU and easy to expand with stacking 
modules. Over the past decade, this PC architecture has become an accepted platform for 
dedicated and embedded applications. PC/104 offers the full architecture, hardware and 
software compatibilities of the standard PC bus, but in compact (3.6 inches x 3.8 inches) 
stackable modules. [Refs. 8 and 14] 

The Real Time Devices PC/104 has powerful features for its size. It operates on 5 
VDC, as do all the other SANS components. Also, this PC/104 has a 12 MB disk ona 
chip (expandable to 72 MB) that will store the SANS code and any data it accumulates. 
A Integral Viper 170 MB hard drive is also available for the PC/104 if more storage is 


required, 1.e. navigation data for post processing. [Ref. 8] 


2. Four RS-232 Port Serial I/O Module 

Two serial ports usually come standard on PC's. However four serial ports were 
needed in SANS. The Sealevel C4-104 serial YO module provides SANS with four serial 
ports each with its own memory addresses and interrupt request assignments. All the 
sensors, the IMU, DGPS, water speed sensor, and electronic compass output to a RS-232 
format. By using this module, an A/D board 1s not necessary to convert analog sensor 
data to a digital representation. Most importantly, removing the A/D board from SANS 
reduces its size by approximately 60 cubic inches. [Ref. 9] 

3. Crossbow DMU-VG Six Axis Inertial Measurement Unit 

The Crossbow DMU-VG six axis IMU consists of three rate gyros, three 
accelerometers, and a 12 bit A/D board all in a 3 x 3.375 x 3.250 inch box. It operates in 
one of two output modes, a scaled sensor mode and a voltage mode. In sensor mode, roll 
and pitch can be obtained directly from an on-board processor. This capability reduces 
the amount of computing for the PC/104. The original SANS code calculated the roll and 
pitch from the angular rates and acceleration readings of the Systron Donner IMU [Ref. 
2]. The Crossbow unit produces a data packet (Table 3.1) which can be used to replace 


this portion in the SANS code. [Ref. 10] 


Note: Most Significant Bit (MSB) Least Significant Bit (LSB) 
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Table 3.1: Crossbow IMU Data Packet Format [Ref. 9] 


4. SonTek Hydra Water Speed Sensor 

The SonTek Hydra water speed sensor is a single point, high resolution, 3D 
Doppler current meter. This device uses one transmitter and 3 acoustic receivers which 
are aligned to intersect with the transmit beam pattern at a common sampling volume. 
The velocity measured by each receiver is referred to as the bistatic velocity, and is the 


projection of the 3D velocity vector on the bistatic axis of the acoustic receiver. An RS- 
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232 output makes intergration with the PC/104 simple. What makes this water sensor 
unique is its ability to report velocity data in the Earth (North-East-Down) coordinate 
system with the aid of an internal compass and tilt sensor. The velocity in Earth 
coordinates is fed directly into the asynchronous Kalman filter described in the next 
chapter. [Ref. 11] 

5. McKinney Technology DGPS Receiver 

This GPS/DGPS receiver package is specially designed to operate in the 
Monterey Bay area. The contained Motorola Oncore receiver is capable of tracking up to 
eight satellites simultaneously. It can provide position accuracy of better than 24 meters 
Spherical Error Probable (SEP) without Selective Availability (SA) and 10 meters (SEP) 
with SA. Typical-Time-To-First Fix (TTFF) is 18 seconds with a reacquisition time of 
2.5 seconds. [Ref. 12] 

6. Precision Navigation TCM2 Electronic Compass Sensor Module 

The TCM2 consists of a three-axis magnetometer and a two-axis tilt sensor with a 
small A/D board to output roll, pitch, heading, and a three dimensional magnetic field 
measurement. It is accurate to within one half of a degree in level operation. Reference 
[5] describes calibration of the compass and its error characteristics. Its size is 2.5 x 2 1.1 
inches. It requires 5 VDC and 15-22 mA. [Ref. 13] 
D. FUTURE SANS HARDWARE 

As of this writing research 1s proceeding regarding other possible components of 
SANS. A method to transmit data from SANS to a remote location is desired. This has 
led to the purchase of the Proxim RangeLAN2 7402 PC card. The RangeLAN2 is 
capable of transmitting data at 1.6 Mbps through a PCMCIA type II card format at 
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distances up to 1000 feet. The SANS PC/104 already comes with a PCMCIA module 
that fit 2 cards. With this feature, data received from the sensors or processed navigation 
information can be observed remotely. [Ref. 15] 

Research into a smaller IMU showed that a device with a three axis 
accelerometer, three axis angular rate sensor, and three axis magnetometer all in a 
package the size of wrist watch is approximately two years away. Demand for such a 
device is slowly increasing, but not fast enough for the commercial industry to mass- 
produce them at this time. Finally, the current SANS DGPS module is three years old. 
Enough advances have been made in the GPS technology that would warrant changing 
out the DGPS package for a system that is smaller and more accurate. Thus far, research 
has offered two possible products. One is Rockwell Semiconductor's NAVCARD LP, 
which comes in a PCMCIA card format [Ref. 16]. The other is Trimble's Pathfinder with 
ASPEN software also in a PCMCIA card format [Ref. 17]. No information was found on 
the DGPS radio receivers integrated with GPS on PCMCIA cards. User defined settings 
of the receiver frequency are set for the area of GPS operation. 

As with any hardware device, its effectiveness is usually determined by the 
quality of its software. The SANS code [Ref. 2] is currently written in C++ and utilizes 
the constant gain complementary filter method found in Chapter II. Using the 
configuration described in this chapter, the next task is to implement an asynchronous 
Kalman filter to integrate the sensor data. The result of this filter work is described in 


Chapter IV. 
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IV. SANS ASYNCHRONOUS KALMAN FILTER 


A. INTRODUCTION 


This chapter discusses the development of a discrete time asynchronous Kalman 
filter that estimates the eight states of the SANS model after attitude estimation. A brief 
discussion will be provided on Kalman filter basics leading into a derivation of the SANS 
Kalman filter equations. Finally, results of the SANS asynchronous Kalman filter Matlab 
simulation will be presented. 

B. KALMAN FILTER BASICS 

The discrete Kalman filter is a recursive predictive update technique used to 
estimate the states of a process model. This state-space filter method has two main 
features (1) vector modeling of the random processes under consideration and (2) 
recursive processing of the noisy measurement (input) data [Ref.1]. Given some initial 
estimates, it allows the states of a model to be predicted and adjusted with each new 
measurement, providing an estimate of error at each update. It incorporates the effects of 
measurement noise and modeling noise in its computational structure. 

A model of a random process can be described as: 

X,.) =O, + Ww, (4.1) 

In cases where the relationship between model and its measurements is linear, the 

observation (measurement) of the random process model (Eq. 4.1) becomes: 


Zz, —H,x, + VY, (4.2) 
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Throughout the remainder of this chapter, various terms are used to better explain the 


Kalman filter. The following notations are consistent with Reference [1]: 


(n x 1) Vector containing actual states of a process model at time , . 
(n x 1) Vector containing the current estimated states at 7, . 
(n x 1) Vector containing the estimated states at time ¢ before updating with 


measurement. 
(n x 1) Vector containing the estimated states at the next time sample. 


(n x n) Matrix containing the error covariance for X, . 

(n x n) Matrix containing the error covariance for x7. 

(n x n) Matrix containing the error covariance for %7,,. 

(m x 1) Vector containing the measurement at time {, . 

(m x n) Matrix giving the ideal (noiseless) connection between the measurement 
and the state vector at time ¢,. 

(m x n) Matrix denoting the measurement error covariance, which must be known 


or estimated a priori. 
(n x n) Matrix containing the model relating &,and x7,, in the absence of a forcing 


function (if %, is a sample of a continuous process, ¢, 1s the usual state transition 
matrix). 

(n x 1) Vector whose elements are white sequences with known covariance 
structure to be used with x, . 

(m x 1) Vector whose elements are the measurement errors associated with z, - 
assumed to be a white sequence with known covariance structure and having zero 
cross-correlation with the w, sequence. 

(n x n) Matrix containing the covariance of w,. 

(n x n) Kalman Gain matrix that relates the amount of influence that the error 
between x; and z, has in deriving x,. 

white noise variable with zero mean and variance of element being modeled. 


The discrete Kalman filter basically contains five recursive equations. Beginning 


with a prior estimate, the noisy measurement z, is used wth a blending factor K, (yet to 


be determined) to improve the the estimate as follows; 


i, = % + K,(z,- H, §) (4.3) 
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K, (known as the Kalman gain) is now needed to find the optimal estimate x,. It 
takes the error covariance (mean-square error) between the current state x, and the 
estimated state x, and applies it with H, and R, resulting in 

Ke — Peo, (HOP H, +R,) (4.4) 

Once the Kalman gain K, minimizes the mean-square estimation error, a new 

covariance matrix can be computed for x, (Eq. 4.3) with 
P, = (I- K,H, )P, (4.5) 

The updated estimate x, is projected ahead using the state transition matrix ¢,. 
Unlike Eq. (4.1) the noise vector w, does not affect the projected states because it has 
zero mean and is white (zero correlation with any previous w, ). Thus, 

Xp = AX, (4.6) 

Finally, the projected error covariance for x;,,, uses the updated error covariance 
P, from Eq. (4.5), the covariance of w, in Eq. (4.1), denoted as Q, , and ¢, the state 
transition matrix to form the following: 

nh = P,P, 9; +Q, (4.7) 


Equations (4.3 — 4.7) can be placed into an algorithm that can loop infinitely. This 


Kalman filter loop is shown in Figure (4.1). 


q) 


> 


q3 


V4 


5 


de 


Enter prior estimate x, and its error covariance P, 


Compute Kalman gain: 


ft Pe 





K, =P, H,(H,P, H, +R,)" 






Project ahead: Update estimate with 


measurement K,: 


as + K, (z, eK; ) 





P,.. =9P.%, + Q, 


Compute error covariance for 
updated estimate: 5 ods 


(I = KH, )P, 





Figure 4.1: Kalman filter loop from Ref. [1] 
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X6 
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Figure 4.2: SANS Process Model (after attitude estimation) 
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C. DERIVATION OF SANS KALMAN FILTER EQUATIONS 

The SANS asynchronous Kalman filter consists of eight states, north position, 
east position, north velocity, east velocity, north current, east current, north GPS bias, and 
east GPS bias. Figure 4.2 shows the process model developed by the SANS team [Ref. 
4]. This model contains the eight states known as the vector xz. Equations 4.8 through 
4.15 show the derivation of the state equations for the SANS process model. 


l l 


4 SG (4.8) 
Ty (si 
l l 
X, =-—x,+—@, (4.9) 
q q 
, l | 
X, =-—-%, + —gq, (4.10) 
(s) T, 
’ l l 
X,=-—x%,+—q, (4.11) 
(2) Tt 
] l 
3 3 
] l 
X, =-—xX,+—4, (4.13) 
t, (z 
ae a a (4.14) 
eX teh (4.15) 


From these equations, @ (state transition matrix) was developed. 
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2 0 0 0 0 0 0 O 
ta 
O et 0 0 0 O O 0 
zn 
O 0 e” O @) O O 0 
4a 
O O 0 e” O O O O 
? = 2a 
O 0 0 O e® O 0 0 
Has 
0) 0 0 0 0 e” 0 0 


The next item needed for the Kalman filter was the Q; matrix. Equation 4.16 is 
the equation for Q,, the mean of model noise squared. 


wz 1S the plant noise with covariance Q,, 


Q, = Elw,w,] (4.16) 


Ze 








21, 
A 
0 —|l-e " 0 0 0 0 0 9) 
2, 
Aes 
0 0 —j}l-e * 0 9) 0 0 9) 
mre 
4 
0 0 0 —l|l-e ” 0 0 0 0 
25, 
Q= 4M 
0 0 0 0 al B | 0 0 0 
aig 
Aa 
0 0 0 0 0 =——||—e 0 0 
Zo 
0 0 0 0 0 0 0 0 
0 0 0 0 0 0 0 0 


and v; 1S measurement noise with covariance R. 


with DGPS signal 


Se Ono wm, 
So Qian © 
So JO 
So iowo 


R= ; 5 without DGPS signal 
O . 


The H matrix is the noiseless connection between measurement and the state vector a 
time ¢. For the SANS asynchronous Kalman filter, two H matrices describe this 


connection, one for samples with DGPS the other for samples without DGPS. 


with DGPS signal 


LS EE I 2) 
pale PR, 2) 


0 
0 
| 
0 


oOo Oo GO = 
oo © =] © 
O'S Oe 
Ooo (oa 
—_- © ODO OO 


= 


) 
H = 


0 
without DGPS signal 
One e021 0) 10a Oi 0, 


Using the main components of the Kalman filter in the previous section, the filter was 
tested in Matlab. 
D. SANS KALMAN FILTER SIMULATION 

The SANS asynchronous Kalman filter was coded in MATLAB® [Ref. 18] by 
applying the matrices developed in section C to the prediction and correction equations as 
outlined in Equations 4.1 through 4.7. Simulation results of the SANS Kalman filter are 
presented in the following section. The source code for the SANS Kalman filter is 
presented in Appendix A. 

The model is assumed stationary throughout the simulation. Noise is generated 
from the process model and measurement errors. The following figures show the results 
of simulation over a six-minute period sampling at a rate of 100 Hz. 

Figure 4.3 shows the north position versus time result in the six-minute 
simulation. The A symbols represent the colored noise sampling of DGPS position. The 
continuous line represents the actual model position, and the dotted line represents the 


estimated Kalman filter position of the model. 
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North Position vs Time 


position (meters) 


Actual Position 
Estimated Position 
Es 4A DGPS Measurement 





0 50 100 150 200 250 300 350 
time (seconds) 


Figure 4.3: Plot of North Position vs. Time 


Notice the initial jumps in the estimated position (dotted line) and how the 
estimate approaches the actual model state within the first 200 seconds although GPS 
information is drifting off. This behavior shows that the Kalman gain for GPS is 


decreasing and more reliance is being given to velocity measurement. 
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East Position vs Time 


Actual Position 
Estimated Position 
DGPS Measurement 


position (meters) 





0 30 100 150 200 250 300 350 
time (seconds) 


Figure 4.4: Plot of East Position vs. Time 


Similar to Figure 4.3, Figure 4.4 displays estimated east position against time. This plot 
demonstrates GPS fluctuating about zero but the Kalman filter is able to maintain a good 
estimate of position given a zero velocity measurement. In addition to plotting position 
versus time, the error covariance matrix P, is presented in Appendix B for examination of 


error covariance and correlation among each of the states in the Kalman filter. 
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Estimated North Position vs Estimated East Position 
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Figure 4.5: Plot of Estimated North Position vs Estimated East Position 


Figure 4.5 represents data from Figures 4.3 and 4.4 and plots them together to 


more geographic plot of the simulation model. The long jumps in position were 


due to initial estimates within the first 30 seconds. As time went on, the position 
differential became minimal. It can be seen that an asynchronous Kalman filter is a good 
way to optimally estimate the states of SANS. This case only presents the position 
information, but all the other states can be easily represented. Much more can be done 
with this simulation, such as including all the states of the SANS filter and getting better 


models for measurement data (GPS and waterspeed). 


oy 











V. CONCLUSIONS 


A. SUMMARY 

The purpose of this thesis was to develop a low cost method to navigate a small 
AUV by integrating an inertial navigation system and differential GPS. Major 
modifications have been made to SANS and the navigation filter now optimally estimates 
position. SANS was reduced by 52% in size and capable of running at an estimated 100 
Hz sampling rate. The components in SANS were selected based on cost, size, and ease 
of operation. An asynchronous Kalman filter was developed and tested in simulation 
with promising results. What makes this navigation filter unique from any other AUV 
navigation system is that it accounts for current. The Kalman filter explained in this 
thesis estimates the AUV position, velocity, ocean current, and DGPS bias. 
B. FUTURE RESEARCH 

The future of SANS has many avenues predicated upon technology advances, 
software development, and the amount of research put into testing and evaluation. As 
micromachined chip technology evolves in the inertial navigation field, improvements in 
IMUs will most certainly come about. Reducing the size of SANS should continually be 
a goal while maintaining performance and low cost. Using the PCMCIA Card format for 
GPS receivers could potentially reduce the size of SANS by another 30 cubic inches. 
The limiting factor with PCMCIA GPS receivers is the added size of a differential signal 
receiver required for DGPS. No DGPS receivers have been implemented onto a single 


board. 


Be 


Addition of the Proxim RangeLAN2 PC card [Ref. 15] gives SANS another 
dimension of usability. This card could give SANS more portability and potentially 
changes the way the sensors are integrated and utilized for applications other than AUVs. 
For example, data coming from the IMU, compass, DGPS, and water speed sensor could 
be transmitted to a remote computer for post processing rather than taking up CPU time 
running the navigation software. 

Now that the asynchronous Kalman filter has been developed, more tests are 
needed to further test its reliability. A better DGPS model could be used for simulations 
that are more realistic. In addition, simulated velocities and currents could provide 
further assurance that the states are being estimated correctly. Another area needing 
work is the navigation software. The original SANS code is in C++ and operated on a 
Maxus ESP 50 MHz PC. A new SANS code needs to work on the PC/104 platform using 
the new IMU and waterspeed data format. The code could be in C++ or Java. Using 
Java gives SANS more flexibility within the network of computers imbedded within the 
Phoenix AUV. 

Currently, research is being done on using quaternions vice Euler angles for 
attitude estimation. Use of quaternions would be necessary for SANS to track the 
motions of human limbs through the vertical. Accomplishing this task would also mean 
reducing the size of SANS to that of a wristwatch, which would be strapped onto various 
body segments. For more information see Reference [19]. Tilt table tests must be 
performed to examine the IMU data. Compass calibration has already been examined 
[Ref. 6]. When SANS reaches the stage when hardware and software are fully integrated 
ground tests and sea trials will be needed to prove its operation. 
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APPENDIX A: SANS ASYNCHRONOUS KALMAN FILTER MATLAB CODES 


clear all 


% SANS Kalman Filter 
% By Glenn Hernandez 
% 8 May 98 


% Number of minutes for simulated run 
minutes = 6; 


% Define the resolution of the simulation here 
delGartu-m.0, /teror I1000HZ resolucion 


% Number of samples 
samples = l/delta t * 60 * minutes; % Gives 60000 samples at delta t = 


-01 seconds 


% Time Constants 


tau_l = 60; % seconds for velocity 
tau_2 = 60; % seconds for GPS 
tau 3 = 3600; % seconds for ocean current 


% Process Noise Vector 


wl = randn(1,samples) ; 

w2 = randn(1,samples); 

w3 = randn(1,samples); 

w4 = randn(1,samples) ; 

wS5S = 600 * randn(1,samples); % Gives a GPS standard deviation of 3 m 
w6 = 600 * randn(1,samples); % Gives a GPS standard deviation of 3 m 
w7 = zeros(1,samples); % No white noise input for x7 

w8 = zeros(1,samples); % No white noise input for x8 

w = [wljw2;w3;w4;wS;w6;w7 ;w8); 


% Measurement Noise Vector 


vl = randn(1,samples); 

v2 = randn(1,samples); 

v3 = randn(1,samples); 

v4 = randn(1,samples); 

v_0 = [vl;v2}; % Noise vector without GPS input 
vi il = [vl;v2;v3;v4]; % Noise vector with GPS input 


% Generate GPS Sampling 
gps_flag = zeros(samples,1); 


for g = 1:1/delta t:samples % Need at least .01 seconds between GPS 
fixes 


j} = rand; % Normalized random generator between O and 1 
Ue  <a05 
gps _flag(g) = 0; % No GPS Signal Available 


3] 


else 
gps flag(g) = 1; % GPS Signal Available 


end 
end 


‘gps flags generated' 


% System Matrix 


A= (eau O 0 6) 0 O 0 
0 1/tau_1 0 0 0 O 0 
6) 0 1/tau_2 O 0 O 0 
0 0 0 Peau 2 O O 0 
0 0 0 0 Ly Laune 0 0 
O O 0 0 O 1/tau_ 3 0 
Vyeaued. + 3C ij taume O 0 0 ) 
0 thy teevel 0 i causZ 0 0 0 
% Input Noise Matrix 
B = [(wl;w2;w3;w4;w5;w6;w7;w8]; 
% Output Matrix 
C = eye(8); 
D = zeros(8,samples); 
% State Transition Matrix 
phi = (exp(-delta _t/tau_1) OCOROF OO 0-0; 
O exp(-delta t/tau 1)9030 50 0°00 ; 
O50 sexp([deltast/ taun2 ) Ogee sO lO; 
0 0 0 exp(-delta t/tau_2) 0 000 ; 
0 0 0 0 exp(-deltact, Paum2) 808010 ; 
00 0 © 0 expisdel tant, tare). 0 0; 
tau t*(1-exp(-deltaat, Caumea o 
tau 2*(1-exp(-deltave, tau) ) 0 0 00 0 ; 
O tau_1*(1l-exp(-delta t/tau_1)) 
O tau_2*(l-exp(-delta t/tau_2)) 0 0 0 Oj; 
OPS, (2 *tausl)) (1 sexpi 24 de lt ame Canal je 0 1020-0, 0 
O ((1/(2*tau_1))*(1-exp((-2*dellta t)/tau_1))) @ 6 0 0 
OO ((1/ (2*tau 2) )*( l-exp (2 -delea t)/tau 2) oe 
O00 ((1/(2*tau 2) )*( d-exp( (—240el tame) Caunc jee 
0000 ((1/(2*tau_3))*(l-exp((-2*delta_t)/tau_3))) 0 
O00 00 0. (ly (2* caus) el -exe((-2*deltame) /tau3))) 
0-0-0 070 0 
00000 0 


% Generate Process Noise Vectors 
process noise = sqrt(Q) *w; 


rror Covariance Matrix 


% E 
RO = diag([.5 .5]); % Without GPS signal 
R_1 = diag((-5 .5 0 0}); % With GPS signal 


by! 


Oooo 000 O@ 


bod 


Ooo o0o00 0 O 


™e¢ “6 “Se 6M OlhUHHUUMCOHUCUMOG CUM 


Ooo 0o0 000 © 


thee! 


i i kL i tT i, TF ein I | 


% Generate Measurement Noise Vectors 
sensor noise 0 = sqrt(R_O) * v_O; % Without GPS signal 
sensor noise 1 = sqrt(R_1) * v_1l; % With GPS signal 


% Initial x_hat_minus 
x hat_minus(8,samples) = zeros; 


% Initial x _hat_plus 
x hat _plus(8,samples) = zeros; 


% Initial State Vector 
x(8,samples) = zeros; 


% Initial Error Covariance Matrix 


Eevinus —o(.5 1 0 00) 10 0 O07; 
O59. 0 0 O FO 0 ; 
(0) 0) 1 b= 5 (0 eal @ SR 9 Ss 0 Saar © at 
O50; “0 800 0 0; 
OF 30) 30) 10" 3) 10250. Oa; 
0°20 00 07 sr 10" 0; 
O06 0170 0 SO, 
O20 56° 0 40 0 70 5); 


timemindexea —"iy" & Initial Index for Measurement Vector without GPS 
time index b = 1; % Initial Index for Measurement Vector with GPS 


"Beginning Kalman Loops' 


% Begin Simulation 
for k = 2:samples 


% Generate State Vectors and Measurement Vectors 
x(:,k) = phi * x(:,k-1) + process noise(:,k-1); 


% Kalman loop with out GPS signal 
if gps flag(k) == 


H= [20000000 ; 
OO" 1.070, 0-0 (O80); 
R = diag([(-5 -.5])); 
sensor _ noise 0(:,k) = sqrt(R) * v_0(:,k); 
z_veli(time index a) = H(1,:) * x(:,k) + sensor noise 0(1,k); 
z_vel2(time_index a) = H(2,:) * x(:,k) + sensor noise 0(2,k); 
z_vel_ time(time_index_a) = k * delta t; 
Zevels—a(zavell jaz vel2Z|; 


% Compute Kalman Gain 
eee eee evn <P minus * H* 't RR); 
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% Update Estimate 
x hat plus(:,k)e= xohat minus :; Kates ke 
7 i (zavel(:, Cime  1nGex ta) 
x hat _minus(:,k-1)); 


% Compute Error Covariance for Updated Estimate 
P plus = ( eye(8) - K * H ) * P_minus; 
Peplus —8()P plusmieeeplus ’ ) oe, 


time _index_a = time! index_a + 1; % Increase the measurement 
vector index by 1 


else 
H= [10 0 0 0°00 702e7, 
OO. 12070, 020 Ose. 
0-0 0°O 1.0 1862: 
O° 0 © 0 Ca err; 
R= diag([.5 .5 ©yV0]); 


Z gpsl(time_index _b) = H(1,:) * x(:,k) + sensor_noise _1(1,k); 
Z gps2(time index _b) = H(2,:) * x(:,k) + Sensor noise 1(2,k); 
Z gps3(time index_b)»= H(3,:) * x(:,k) + Sensor noise 1(3,k); 
Zz gps4(time index b) = H(4,:) * x(:,k) + sensor_noise 1(4,k); 


z gps = [2 gpsl ; Gagesz2 =, < 9PsS. , z gps4)j; 
Z Ops time (time side meee ~ deltarr, 


% Compute Kalman Gain 
K = P minus * 8° * anv(2e* Po ominusp* Ho + R); 


% Update Estimate 

x Hat plus(:,k) =" xehadeeminus(:,k=1) 7 Ke* 
(ZEgpsG, -ime 1ndexa> ma has 
x hat minus(:,k-1)); 

% Compute Error Covariance for Updated Estimate 

P_plus’= (eyes) = han) -F  minuc: 

P plus®= (PF plus 7 eeeeus sj, 2; 


time index b = timevindexeb> + 1; 5% increnise the measurement 
vector index by 1 


end 


pmin(:,:,k) = P_plus; % Save Matrix for future analysis 
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% Project Ahead 
x hat minus(:,k) = phi * x_hat_plus(:,k); 
Peniunvoe— Puig ere plus: ~ pii 9+ 0; 


P minus = ( P minus + P_minus' ) / 2; 

time(k) = k * delta t; % Memorize time index 
end 
figure(1) 


subplot(3,1,1) 

plot(time, x(7,:), ‘b-',time, x_hat_minus(7,:), ‘r-.',Z gps time, 
Zz gps3, 'g°') 

xlabel('time (seconds)') 

ylabel('position (meters)') 

title( 'North Position vs Time') 

axis({O max(time) -1.5*max(abs(z gps3)) 1.5*max(abs(z_ gps3))])) 


subplet( 3, 1,2) 

Peoeitime, x(S,:), D= ,time, x hat minus(S,:), “r--.",z gps time, 
z_ gps4, 'g°') 

xlabel('time (seconds)') 

ylabel('position (meters) ') 

title( 'East Position vs Time') 

axis(([O max(time) ~-1.5*max(abs(z gps4)) 1.5*max(abs(z_ gps4)))) 


subplot(3,1,3) 

plot(x_hat_minus(7,:),x_hat_minus(8,:),'b-') 

xlabel('North Position’) 

ylabel('East Position') 

title('North-East Position Plot') 

axis((-1.5*max(abs(x_hat_minus(7,:))) 1.5*max(abs(x_hat_minus(7,:)))... 
-1.5*max(abs(x hat _minus(8,:))) 1.5*max(abs(x_hat_minus(8,:)))]) 


orient tall 
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APPENDIX B: ERROR COVARIANCE MATRIX FOR SANS 
ASYNCHRONOUS KALMAN FILTER SIMULATION 
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8 x 8 Matrix for Error Covariance P in SANS Kalman Filter Simulation 
Each box represents one element of the matrix vs time 
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